


#include "FreeRTOS.h"
#include "queue.h"

#include "state.h"
#include "config.h"

#include "qmath.h"


//=====================================================================================================
// MadgwickAHRS.c
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date			Author          Notes
// 29/09/2011	SOH Madgwick    Initial release
// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
// 19/02/2012	SOH Madgwick	Magnetometer measurement is normalised
//
//=====================================================================================================

//---------------------------------------------------------------------------------------------------
// Header files

//---------------------------------------------------------------------------------------------------
// Definitions

#define betaDef		FLOAT_TO_Q(0.1f)		// 2 * proportional gain

//---------------------------------------------------------------------------------------------------
// Variable definitions

q_t beta = betaDef;								// 2 * proportional gain (Kp)
q_t q0 = FLOAT_TO_Q(1.0f);
q_t q1 = FLOAT_TO_Q(0.0f);
q_t q2 = FLOAT_TO_Q(0.0f);
q_t q3 = FLOAT_TO_Q(0.0f);	// quaternion of sensor frame relative to auxiliary frame


void quatReset()
{
    beta = betaDef;
    q0 = FLOAT_TO_Q(1.0f);
    q1 = FLOAT_TO_Q(0.0f);
    q2 = FLOAT_TO_Q(0.0f);
    q3 = FLOAT_TO_Q(0.0f);
}
//====================================================================================================
// Functions

//---------------------------------------------------------------------------------------------------
// AHRS algorithm update

void quaternion()
{
	q_t recipNorm;
	q_t s0, s1, s2, s3;
	q_t qDot1, qDot2, qDot3, qDot4;
	q_t hx, hy;
	q_t _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _8bx, _8bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
	//q32_t _4q0, _4q1, _4q2, _8q1, _8q2;

	static q_vector_t g,a,m;
//	g.x = STATE.imu.gyroScaledToSec.x;
//	g.y = STATE.imu.gyroScaledToSec.y;de
//	g.z = STATE.imu.gyroScaledToSec.z;

    g.x = CONFIG.imu.quatGyroSetup[0] == 0 ? 0 :
          CONFIG.imu.quatGyroSetup[0] > 0 ? qMul(((q_t*)&STATE.imu.gyroScaledToSec)[CONFIG.imu.quatGyroSetup[0]-1], Q_DEG_TO_RAD*CONFIG.imu.freq) :
          qMul(((q_t*)&STATE.imu.gyroScaledToSec)[-CONFIG.imu.quatGyroSetup[0]-1], Q_DEG_TO_RAD*CONFIG.imu.freq);

    g.y = CONFIG.imu.quatGyroSetup[1] == 0 ? 0 :
          CONFIG.imu.quatGyroSetup[1] > 0 ? qMul(((q_t*)&STATE.imu.gyroScaledToSec)[CONFIG.imu.quatGyroSetup[1]-1], Q_DEG_TO_RAD*CONFIG.imu.freq) :
          qMul(((q_t*)&STATE.imu.gyroScaledToSec)[-CONFIG.imu.quatGyroSetup[1]-1], Q_DEG_TO_RAD*CONFIG.imu.freq);

    g.z = CONFIG.imu.quatGyroSetup[2] == 0 ? 0 :
          CONFIG.imu.quatGyroSetup[2] > 0 ? qMul(((q_t*)&STATE.imu.gyroScaledToSec)[CONFIG.imu.quatGyroSetup[2]-1], Q_DEG_TO_RAD*CONFIG.imu.freq) :
          qMul(((q_t*)&STATE.imu.gyroScaledToSec)[-CONFIG.imu.quatGyroSetup[2]-1], Q_DEG_TO_RAD*CONFIG.imu.freq);

	a.x = CONFIG.imu.quatAccSetup[0] == 0 ? 0 :
          CONFIG.imu.quatAccSetup[0] > 0 ? ((q_t*)&STATE.imu.accFilteredTransposed)[CONFIG.imu.quatAccSetup[0]-1] :
              ((q_t*)&STATE.imu.accFilteredTransposed)[-CONFIG.imu.quatAccSetup[0]-1];
	a.y = CONFIG.imu.quatAccSetup[1] == 0 ? 0 :
          CONFIG.imu.quatAccSetup[1] > 0 ? ((q_t*)&STATE.imu.accFilteredTransposed)[CONFIG.imu.quatAccSetup[1]-1] :
              ((q_t*)&STATE.imu.accFilteredTransposed)[-CONFIG.imu.quatAccSetup[1]-1];
	a.z = CONFIG.imu.quatAccSetup[2] == 0 ? 0 :
          CONFIG.imu.quatAccSetup[2] > 0 ? ((q_t*)&STATE.imu.accFilteredTransposed)[CONFIG.imu.quatAccSetup[2]-1] :
              ((q_t*)&STATE.imu.accFilteredTransposed)[-CONFIG.imu.quatAccSetup[2]-1];

    m.x = CONFIG.imu.quatMagnSetup[0] == 0 ? 0 :
          CONFIG.imu.quatMagnSetup[0] > 0 ? ((q_t*)&STATE.imu.magnFiltered)[CONFIG.imu.quatMagnSetup[0]-1] :
              ((q_t*)&STATE.imu.magnFiltered)[-CONFIG.imu.quatMagnSetup[0]-1];
	m.y = CONFIG.imu.quatMagnSetup[1] == 0 ? 0 :
          CONFIG.imu.quatMagnSetup[1] > 0 ? ((q_t*)&STATE.imu.magnFiltered)[CONFIG.imu.quatMagnSetup[1]-1] :
              ((q_t*)&STATE.imu.magnFiltered)[-CONFIG.imu.quatMagnSetup[1]-1];
	m.z = CONFIG.imu.quatMagnSetup[2] == 0 ? 0 :
          CONFIG.imu.quatMagnSetup[2] > 0 ? ((q_t*)&STATE.imu.magnFiltered)[CONFIG.imu.quatMagnSetup[2]-1] :
              ((q_t*)&STATE.imu.magnFiltered)[-CONFIG.imu.quatMagnSetup[2]-1];

    if (CONFIG.imu.quatGyroSetup[0] < 0) g.x = -g.x;
    if (CONFIG.imu.quatGyroSetup[1] < 0) g.y = -g.y;
    if (CONFIG.imu.quatGyroSetup[2] < 0) g.z = -g.z;
    if (CONFIG.imu.quatMagnSetup[0] < 0) m.x = -m.x;
    if (CONFIG.imu.quatMagnSetup[1] < 0) m.y = -m.y;
    if (CONFIG.imu.quatMagnSetup[2] < 0) m.z = -m.z;
    if (CONFIG.imu.quatAccSetup[0] < 0) a.x = -a.x;
    if (CONFIG.imu.quatAccSetup[1] < 0) a.y = -a.y;
    if (CONFIG.imu.quatAccSetup[2] < 0) a.z = -a.z;

	// Rate of change of quaternion from gyroscope
	qDot1 = qMul(Q_HALF, (qMul(-q1,g.x) - qMul(q2, g.y) - qMul(q3, g.z)));
	qDot2 = qMul(Q_HALF, (qMul(q0,g.x) + qMul(q2, g.z) - qMul(q3, g.y)));
	qDot3 = qMul(Q_HALF, (qMul(q0,g.y) - qMul(q1, g.z) + qMul(q3, g.x)));
	qDot4 = qMul(Q_HALF, (qMul(q0,g.z) + qMul(q1, g.y) - qMul(q2, g.x)));

	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if(!((a.x == 0) && (a.y == 0) && (a.z == 0)))
	{
		// Normalise accelerometer measurement
		vectorNormalize_inline(a);

		// Auxiliary variables to avoid repeated arithmetic
		_2q0 = q0 * 2;
		_2q1 = q1 * 2;
		_2q2 = q2 * 2;
		_2q3 = q3 * 2;
		q0q0 = qPow2(q0);
		q1q1 = qPow2(q1);
		q2q2 = qPow2(q2);
		q3q3 = qPow2(q3);

		// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
		if ((m.x == 0) && (m.y == 0) && (m.z == 0))
		{
			// Gradient decent algorithm corrective step
			s0 = 4*qMul(q0,q2q2) + qMul(_2q2, a.x) + 4*qMul(q0, q1q1) - qMul(_2q1,a.y);
			s1 = 4*qMul(q1,q3q3) - qMul(_2q3, a.x) + 4*qMul(q0q0, q1) - qMul(_2q0,a.y) - 4*q1 + 8*qMul(q1, q1q1) + 8*qMul(q1, q2q2) + 4*qMul(q1,a.z);
			s2 = 4*qMul(q0q0,q2) + qMul(_2q0, a.x) + 4*qMul(q2, q3q3) - qMul(_2q3,a.y) - 4*q2 + 8*qMul(q2, q1q1) + 8*qMul(q2, q2q2) + 4*qMul(q2,a.z);
			s3 = 4*qMul(q1q1,q3) - qMul(_2q1, a.x) + 4*qMul(q2q2, q3) - qMul(_2q2,a.y);
		}
		else
		{
			// Normalise magnetometer measurement
            vectorNormalize_inline(m);
            
			// Auxiliary variables to avoid repeated arithmetic
			_2q0mx = 2 * qMul(q0, m.x);
			_2q0my = 2 * qMul(q0, m.y);
			_2q0mz = 2 * qMul(q0, m.z);
			_2q1mx = 2 * qMul(q1, m.x);
			_2q0q2 = 2 * qMul(q0, q2);
			_2q2q3 = 2 * qMul(q2, q3);
			q0q1 = qMul(q0, q1);
			q0q2 = qMul(q0, q2);
			q0q3 = qMul(q0, q3);
			q1q2 = qMul(q1, q2);
			q1q3 = qMul(q1, q3);
			q2q3 = qMul(q2, q3);

            // Reference direction of Earth's magnetic field
            hx = qMul(m.x, q0q0) - qMul(_2q0my, q3) + qMul(_2q0mz, q2) + qMul(m.x, q1q1) + qMul3(_2q1, m.y, q2) + qMul3(_2q1, m.z, q3) - qMul(m.x, q2q2) - qMul(m.x, q3q3);
            hy = qMul(_2q0mx, q3) + qMul(m.y, q0q0) - qMul(_2q0mz, q1) + qMul(_2q1mx, q2) - qMul(m.y, q1q1) + qMul(m.y, q2q2) + qMul3(_2q2, m.z, q3) - qMul(m.y, q3q3);
            _2bx = qSqrt(qMul(hx, hx) + qMul(hy, hy));
            _2bz = qMul(-_2q0mx, q2) + qMul(_2q0my, q1) + qMul(m.z, q0q0) + qMul(_2q1mx, q3) - qMul(m.z, q1q1) + qMul3(_2q2, m.y, q3) - qMul(m.z, q2q2) + qMul(m.z, q3q3);
            _4bx = 2 * _2bx;
            _4bz = 2 * _2bz;
            _8bx = 2 * _4bx;
            _8bz = 2 * _4bz;

            // Gradient decent algorithm corrective step
            s0= -qMul(_2q2,(2*(q1q3 - q0q2) - a.x))    +   qMul(_2q1,(2*(q0q1 + q2q3) - a.y))   +  -qMul3(_4bz,q2,(qMul(_4bx,(Q_HALF - q2q2 - q3q3)) + qMul(_4bz,(q1q3 - q0q2)) - m.x))   +   qMul((qMul(-_4bx,q3)+qMul(_4bz,q1)),(qMul(_4bx,(q1q2 - q0q3)) + qMul(_4bz,(q0q1 + q2q3)) - m.y))    +   qMul3(_4bx,q2,(qMul(_4bx,(q0q2 + q1q3)) + qMul(_4bz,(Q_HALF - q1q1 - q2q2)) - m.z));
            s1= qMul(_2q3,(2*(q1q3 - q0q2) - a.x))     +   qMul(_2q0,(2*(q0q1 + q2q3) - a.y))   +  -qMul3(4,q1,(2*(Q_HALF - q1q1 - q2q2) - a.z))    +   qMul3(_4bz,q3,(qMul(_4bx,(Q_HALF - q2q2 - q3q3)) + qMul(_4bz,(q1q3 - q0q2)) - m.x))   + qMul((qMul(_4bx,q2)+qMul(_4bz,q0)),(qMul(_4bx,(q1q2 - q0q3)) + qMul(_4bz,(q0q1 + q2q3)) - m.y))   +   qMul((qMul(_4bx,q3)-qMul(_8bz,q1)),(qMul(_4bx,(q0q2 + q1q3)) + qMul(_4bz,(Q_HALF - q1q1 - q2q2)) - m.z));
            s2= -qMul(_2q0,(2*(q1q3 - q0q2) - a.x))    +
                qMul(_2q3,(2*(q0q1 + q2q3) - a.y))   +
                qMul((-4*q2),(2*(Q_HALF - q1q1 - q2q2) - a.z)) +
                qMul((qMul(-_8bx,q2)-qMul(_4bz,q0)),(qMul(_4bx,(Q_HALF - q2q2 - q3q3)) + qMul(_4bz,(q1q3 - q0q2)) - m.x))+
                qMul((qMul(_4bx,q1)+(qMul(_4bz,q3))),(qMul(_4bx,(q1q2 - q0q3)) + qMul(_4bz,(q0q1 + q2q3)) - m.y))+
                qMul((qMul(_4bx,q0)-qMul(_8bz,q2)),(qMul(_4bx,(q0q2 + q1q3)) + qMul(_4bz,(Q_HALF - q1q1 - q2q2)) - m.z));
            s3= qMul(_2q1,(2*(q1q3 - q0q2) - a.x))     +
                qMul(_2q2,(2*(q0q1 + q2q3) - a.y))   +
                qMul((qMul(-_8bx,q3)+qMul(_4bz,q1)),(qMul(_4bx,(Q_HALF - q2q2 - q3q3)) + qMul(_4bz,(q1q3 - q0q2)) - m.x))+
                qMul((qMul(-_4bx,q0)+qMul(_4bz,q2)),(qMul(_4bx,(q1q2 - q0q3)) + qMul(_4bz,(q0q1 + q2q3)) - m.y))+
                qMul((qMul(_4bx,q1)),(qMul(_4bx,(q0q2 + q1q3)) + qMul(_4bz,(Q_HALF - q1q1 - q2q2) - m.z)));
        }
		
		q_t ss = qSqrt(qPow2(s0) + qPow2(s1) + qPow2(s2) + qPow2(s3));
		if (ss != 0)
		{
			recipNorm = qDiv(Q_ONE,ss); // normalise step magnitude
			s0 = qMul(s0,recipNorm);
			s1 = qMul(s1,recipNorm);
			s2 = qMul(s2,recipNorm);
			s3 = qMul(s3,recipNorm);

			// Apply feedback step
			qDot1 -= qMul(beta,s0);
			qDot2 -= qMul(beta,s1);
			qDot3 -= qMul(beta,s2);
			qDot4 -= qMul(beta,s3);
		}
		else
		{
			portNOP();
		}
	}

	q_t dt = Q_ONE / CONFIG.imu.freq;
	// Integrate rate of change of quaternion to yield quaternion
	q0 += qMul(qDot1, dt);
	q1 += qMul(qDot2, dt);
	q2 += qMul(qDot3, dt);
	q3 += qMul(qDot4, dt);

	// Normalise quaternion
	recipNorm = qDiv(Q_ONE,qSqrt(qPow2(q0) + qPow2(q1) + qPow2(q2) + qPow2(q3)));
	q0 = qMul(q0,recipNorm);
	q1 = qMul(q1,recipNorm);
	q2 = qMul(q2,recipNorm);
	q3 = qMul(q3,recipNorm);

	q_t qpitch = q32asin(2*(qMul(q0,q2)-qMul(q3,q1)));
	q_t qroll = q32atan2(2*(qMul(q0,q1)+qMul(q2,q3)),Q_ONE-2*(qMul(q1,q1)+qMul(q2,q2)));
	q_t qyaw = q32atan2(2*(qMul(q0,q3)+qMul(q1,q2)),Q_ONE-2*(qMul(q2,q2)+qMul(q3,q3)));

	STATE.imu.output.pitch = qMul(qpitch,Q_RAD_TO_DEG);
	STATE.imu.output.roll = qMul(qroll,Q_RAD_TO_DEG);

    /*
    btDebug(STATE.imu.output.pitch >> 24);
    btDebug(STATE.imu.output.pitch >> 16);
    btDebug(STATE.imu.output.pitch >> 8);
    btDebug(STATE.imu.output.pitch & 0xFF);

    btDebug(STATE.imu.output.roll >> 24);
    btDebug(STATE.imu.output.roll >> 16);
    btDebug(STATE.imu.output.roll >> 8);
    btDebug(STATE.imu.output.roll & 0xFF);
*/
	STATE.imu.output.yaw = qMul(qyaw,Q_RAD_TO_DEG);
	STATE.imu.output.pitchD = STATE.imu.gyroScaledToSec.x; // / CONFIG.imu.freq;
	STATE.imu.output.rollD = STATE.imu.gyroScaledToSec.y; // / CONFIG.imu.freq;
	STATE.imu.output.yawD = STATE.imu.gyroScaledToSec.z; // / CONFIG.imu.freq;

}

